from gameobjects.angle3 import Angle3
from gameobjects.matrix44 import Matrix44
from gameobjects.vector3 import Vector3
from phuce.timing import FrameTime


class Movement(object):

  ACCELERATION = 10.0
  FRICTION = 6.0
  STOP_SPEED = 100.0

  def __init__(self, orientation):
    self.orientation = orientation
    self.velocity = Vector3()

  def accelerate(self, speed, direction):
    delta = speed - self.velocity.dot(direction)
    if delta > 0:
      self.velocity += direction * min(delta, speed * self.ACCELERATION * FrameTime.seconds)

  def friction(self):
    ''' Apply friction to the current velocity. '''
    speed = Vector3(self.velocity.x, 0, self.velocity.z).length
    if speed < 1:
      self.velocity.x, self.velocity.y = 0, 0
      return
    loss = max(speed, self.STOP_SPEED) * self.FRICTION * FrameTime.seconds
    self.velocity *= max(0, speed - loss) / speed

  def update(self, bsp, target_velocity):
    self.friction()
    target_speed = target_velocity.length
    if target_speed > 0:
      self.accelerate(target_speed, target_velocity.get_normalized())
    seconds = FrameTime.seconds
    for i in range(4):
      trace = bsp.trace(start=self.orientation.xyz, end=self.orientation.xyz + self.velocity * seconds, contents_mask=1)
      self.orientation.xyz = trace.end.copy()
      if trace.fraction == 1:
        break
      seconds -= seconds * trace.fraction
      if trace.plane:
        f = trace.plane.normal.dot(self.velocity)
        self.velocity -= trace.plane.normal * (f < 0 and f * 1.001 or f / 1.001)


class Orientation(object):

  def __init__(self, xyz=None, ypr=None):
    self.xyz = xyz or Vector3()
    self.ypr = ypr or Angle3()
    self.backup()

  def backup(self):
    self.previous_xyz = self.xyz.copy()
    self.previous_ypr = self.ypr.copy()

  def get_forward(self):
    return self.ypr_matrix.forward.as_vec3()
  forward = property(get_forward)

  def get_right(self):
    return self.ypr_matrix.right.as_vec3()
  right = property(get_right)

  def get_up(self):
    return self.ypr_matrix.up.as_vec3()
  up = property(get_up)

  def get_ypr(self):
    return self._ypr
  def set_ypr(self, ypr):
    self._ypr = Angle3(ypr)
    self._ypr.normalize180()
    self.ypr_matrix  = Matrix44.y_rotation(self._ypr.yaw)
    self.ypr_matrix *= Matrix44.x_rotation(self._ypr.pitch)
  ypr = property(get_ypr, set_ypr)

  def rotate(self, ypr):
    self.ypr = self.ypr + ypr

  def translate(self, xyz, x_axis=Vector3(1, 0, 0), y_axis=Vector3(0, 1, 0), z_axis=Vector3(0, 0, 1)):
    self.xyz += (x_axis * xyz.x) + (y_axis * xyz.y) + (z_axis * xyz.z)
